#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <errno.h>
#include <unistd.h>
#include <sys/time.h> 
#include "xlinkHS.h"
#include "rtk.h"

/**
  ******************************************************************************
  * @file           : rtk.c
  * @brief          : implement for rtk.c file.
  *                   This file contains the common implement of the application.
  * @author			: ac
  * @attention
  *
  *		Copyright (c) 2023 Technology Co., Ltd.
  *
  *******************************************************************************
  */


/*
 * globla variable
 */


/*
 * function declaration
 */

XAG_U8 Data_CheckSum(XAG_U8 *data, XAG_U16 len)
{
    XAG_U8 checksum = 0;
    while (len--) {
        checksum += *data++;
    }
    return checksum;
}

void rtk_cmd_package(unsigned char *protocol_buff, int len, int cmd)
{
	Rtk_0x5B_Header_T head = {0};
	int i = 0, pro_len = 0;

	head.u8Start = FRAME_START;
	head.u16Length = ((len)&0xff) | (((len)>>8)&0xff);
	head.u32ProtocolVersion = (((PROTOCOL_VERSION)&0xff)) | 
							  ((((PROTOCOL_VERSION)>>8)&0xff) << 8) | 
							  ((((PROTOCOL_VERSION)>>16)&0xff) << 16) | 
							  ((((PROTOCOL_VERSION)>>24)&0xff) << 24);
	head.u8Status = 0;
	head.u8Cmd = cmd;
	//head.u8Checksum = Data_CheckSum((unsigned char*)&head, sizeof(Rtk_0x5B_Header_T));
    head.u8Checksum = 0x02;

	memcpy(protocol_buff, (char *)&head, sizeof(Rtk_0x5B_Header_T));

	pro_len = sizeof(Rtk_0x5B_Header_T);
	#if 0
	for (i = 0; i < pro_len; i++) {
		printf("%.2x ",protocol_buff[i]);
	}
	printf("\n0x5B len = %d\n", pro_len);
	#endif
}

void rtk_resolving_0x02_cmd(unsigned char * pdata, int len)
{
	Station_Status_T *status;

	status = (Station_Status_T *) &pdata[0];

	printf("Fix_mode = %d\n", status->Fix_mode);
	printf("Longitude = %lf\n", status->Longitude);
	printf("Latitude = %lf\n", status->Latitude);
	printf("Altitude = %lf\n", status->Altitude);
	printf("PosAccuracy = %lf\n", status->PosAccuracy);
	printf("HgtAccuracy = %lf\n", status->HgtAccuracy);
	printf("Undulation = %lf\n", status->Undulation);
}

void rtk_resolving_0x25_cmd(unsigned char * pdata, int len)
{
	Gps_Event_T * event = NULL;

	event = (Gps_Event_T *) &pdata[0];

	printf("week = %d\n", event->time.week);
	printf("counter = %d\n", event->time.counter);
	printf("second = %d\n", event->time.second);
	printf("subSecond = %d\n", event->time.subSecond);
	printf("offset_second = %d\n", event->time.offset_second);
	printf("offset_subSecond = %d\n", event->time.offset_subSecond);

	printf("lat = %lf\n", event->pos.lat);
	printf("lon = %lf\n", event->pos.lon);
	printf("hgt = %lf\n", event->pos.hgt);
	printf("lat_sd = %lf\n", event->pos.lat_sd);
	printf("lon_sd = %lf\n", event->pos.lon_sd);
	printf("hgt_sd = %lf\n", event->pos.hgt_sd);

	printf("eastVel = %lf\n", event->vel.eastVel);
	printf("northVel = %lf\n", event->vel.northVel);
	printf("upVel = %lf\n", event->vel.upVel);

	printf("FixMode = %d\n", event->sol.FixMode);
	printf("stn_id = %d\n", event->sol.stn_id);
	printf("diff_age = %d\n", event->sol.diff_age);
	printf("solnSVs = %d\n", event->sol.solnSVs);
}
